%% 绘制地图，障碍物，起点终点，路径以及车辆动画（若有）
function VehicleAnimation(x,y,theta,cfg,veh,animation)
    % 设置图像大小
    figure();

    % 设置图像坐标轴
    axis equal;
    xlim([cfg.MINX,cfg.MAXX]);
    ylim([cfg.MINY,cfg.MAXY]);

    % 绘制障碍物
    ObstList = cfg.OBSTACLE;
    for i = 1:length(ObstList)
        j = ObstList(i,1);
        k = ObstList(i,2);
        obs_x = [j-1, j-1, j, j, j-1];
        obs_y = [k-1, k, k, k-1, k-1];
        fill(obs_x, obs_y,'k');
        hold on;
    end

    % 设置图像坐标轴
    axis equal;
    xlim([cfg.MINX,cfg.MAXX]);
    ylim([cfg.MINY,cfg.MAXY]);

    % 绘制路径
    if ~isempty(x)
        plot(x,y,'b'); % 规划出来的轨迹，蓝色曲线
        hold on;
    end

    % 设置图像坐标轴
    axis equal;
    xlim([cfg.MINX,cfg.MAXX]);
    ylim([cfg.MINY,cfg.MAXY]);

    % 绘制起始点
    start = cfg.Start;
    px = start(1);
    py = start(2);
    pth = start(3);
    plot(px,py,'bx','MarkerSize',6);
    hold on;
    if ~animation
        [vehx,vehy] = getVehTran(px,py,pth,veh);
        plot(vehx,vehy,'k');
        hold on;
    end

    % 绘制结束点
    goal = cfg.End;
    px = goal(1);
    py = goal(2);
    pth = goal(3);
    plot(px,py,'bo','MarkerSize',6);
    hold on;
    if ~animation
        [vehx,vehy] = getVehTran(px,py,pth,veh);
        plot(vehx,vehy,'k');
        hold on;
    end

    % 设置图像坐标轴
    axis equal;
    xlim([cfg.MINX,cfg.MAXX]);
    ylim([cfg.MINY,cfg.MAXY]);
    
    % 绘制路径和车辆动画
    if ~isempty(x) && animation
        % 设置图像大小
        sz=get(0,'screensize');
        hFig = gcf; % 获取当前图的句柄
        set(hFig, 'outerposition', sz); % 设置当前图的位置属性

        px = x(1);
        py = y(1);
        pth = theta(1);
        [vehx,vehy] = getVehTran(px,py,pth,veh); % 根据后轴中心的位姿计算车辆边框的位姿
        h1 = plot(vehx,vehy,'k'); % 车辆边框
        h2 = plot(px,py,'rx','MarkerSize',10); % 车辆后轴中心
        img = getframe(gcf);
        videoFWriter = VideoWriter('Parking.mp4','MPEG-4');
        open(videoFWriter);
        writeVideo(videoFWriter,img);
        for i = 2:length(theta)
            px = x(i);
            py = y(i);
            pth = theta(i);
            [vehx,vehy] = getVehTran(px,py,pth,veh);
            h1.XData = vehx; % 更新h1图像句柄,把车辆边框四个角点的x坐标添加进去
            h1.YData = vehy;
            h2.XData = px; % 更新h2图像句柄,把车辆边框四个角点的y坐标添加进去
            h2.YData = py;
            img = getframe(gcf);
            writeVideo(videoFWriter,img);
        end
        close(videoFWriter);
    end
end

 % 根据后轴中心的位姿计算车辆边框的位姿
function [x,y] = getVehTran(x,y,theta,veh)
    % 车辆尺寸
    W = veh.W;
    LF = veh.LF;
    LB = veh.LB;

    % 四个角的位置
    Cornerfl = [LF, W/2]; % 左前方角点
    Cornerfr = [LF, -W/2]; % 右前方角点
    Cornerrl = [-LB, W/2]; % 左后方角点
    Cornerrr = [-LB, -W/2]; % 右后方角点

    % 位置和方向
    Pos = [x,y]; % 后轴中心坐标
    dcm = angle2dcm(-theta, 0, 0); % 计算四个角点的旋转矩阵,由于是刚体的一部分，旋转矩阵相同，将角度转换为方向余弦矩阵，旋转顺序是ZYX

    % 获取四个角的全局坐标
    tvec = dcm*[Cornerfl';0]; % 旋转变换，Cornerfl旋转后形成的列向量，位置向量3*1，最后一个是z坐标
    tvec = tvec';
    Cornerfl = tvec(1:2)+Pos; % 平移变换
    
    tvec = dcm*[Cornerfr';0];
    tvec = tvec';
    Cornerfr = tvec(1:2)+Pos;
    
    tvec = dcm*[Cornerrl';0];
    tvec = tvec';
    Cornerrl = tvec(1:2)+Pos;
    
    tvec = dcm*[Cornerrr';0];
    tvec = tvec';
    Cornerrr = tvec(1:2)+Pos;
    
    % 返回车辆边框四个角点的x,y坐标
    x = [Cornerfl(1),Cornerfr(1),Cornerrr(1),Cornerrl(1),Cornerfl(1)];
    y = [Cornerfl(2),Cornerfr(2),Cornerrr(2),Cornerrl(2),Cornerfl(2)];
end